/*
 * Guidance.cpp
 *
 *  Created on: Jun 9, 2011
 *      Author: tflanzer
 */

#include "Guidance.h"

// Define constants
const int izero = 0;
const int ione = 1;
const double dzero = 0;
const double done = 1;
const double pi = 3.1415926535897932384626433832;

Guidance::Guidance(FileWriter *fwi, int fg) {

	// Load text file of gains/parameters
	loadGains();

	// Circle, altitude, velocity, parameters
	Nc     = ctrlGns.cmap["Nc"];
	Ec     = ctrlGns.cmap["Ec"];
	Rcirc  = ctrlGns.cmap["Rcirc"];
	dDes   = ctrlGns.cmap["dDes"];
	altDev = ctrlGns.cmap["altDev"];
	vDev   = ctrlGns.cmap["vDev"];
	Vtarget = ctrlGns.cmap["Vtarget"];

	// Ground station ID
	fgnd = fg;

	// Initialize number of waypoints
	pv.numwps = 40;

	// Compute waypoint locations and Bezier track vectors
	computeWaypointTracks();

	// Record initial set of waypoints into wp0
	for (int i = 0; i < pv.numwps+1; i++) {
		wp0[0][i] = wp[0][i];
		wp0[1][i] = wp[1][i];
		wp0[2][i] = wp[2][i];
	}

	// Set target velocity
	for (int i = 0; i < pv.numwps+1; i++) {
		pv.waypointVel[i] = Vtarget;//-(vDev*cos(8*pi*double((i-1))/pv.numwps));
	}

	// Initialize previous value of u and last waypoint index
	ul = 0.0;
	wpl = 0;

	// Set lookahead time ta
	ta = ctrlGns.cmap["tahead"];
	L1 = ctrlGns.cmap["L1gain"];

	// Initialize filewriter
	fw = fwi;
}

Guidance::~Guidance() {

}

double Guidance::getBankAngleCmd() {

	// Local variables
	int idx[2], idxtp[2];
	double p[3], ru[3], nru[3], ploc[3], tploc[3], dBdu[3], tp[3], ndBdu,
	       v1[3], v2[3], v1n, v2n, tmp[3], eta, V, phides;

	// Get current position
	p[0] = sv.Np;
	p[1] = sv.Ep;
	p[2] = 0;

	// Get current gain settings
	ta = ctrlGns.cmap["tahead"];
	L1 = ctrlGns.cmap["L1gain"];

	// Find nearest waypoint indicies
	computeWaypointNumber(p,idx);
	fw->updateValue("wpidx1",idx[0]);
	fw->updateValue("wpidx2",idx[1]);

	// Check to see if a new waypoint has been reached
	if (pv.wpidx[1] != idx[1]) {
		mavlink_message_t msgdwn;
		uint8_t bufdwn[MAVLINK_MAX_PACKET_LEN];
		mavlink_msg_waypoint_reached_pack(100, 1, &msgdwn, (uint16_t) idx[1]);
		uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msgdwn);
		write(fgnd,bufdwn,len);
		cout << "Waypoint # " << idx[1] << " reached." << endl;
	}
	pv.wpidx[0] = idx[0];
	pv.wpidx[1] = idx[1];

	// Compute location along waypoint segment
	findPseudoLoc(p,idx,ploc,ru,nru);

	fw->updateValue("ploc1",ploc[0]);
	fw->updateValue("ploc2",ploc[1]);

	// Get tangent at position along trajectory
	computeCurvature(idx,dBdu);

	// Compute target point, assumed to be V*ta ahead, tangent to pseudo-loc
	//V = 1.0*sqrt(sv.VNi*sv.VNi+sv.VEi*sv.VEi+sv.VDi*sv.VDi);
	ndBdu = sqrt(dBdu[0]*dBdu[0]+dBdu[1]*dBdu[1]+dBdu[2]*dBdu[2]); // norm
	tp[0] = ploc[0] + dBdu[0]/ndBdu*L1;
	tp[1] = ploc[1] + dBdu[1]/ndBdu*L1;
	tp[2] = ploc[2] + dBdu[2]/ndBdu*L1;

	// Compute waypoint incides that the target point is between on current trajectory
	computeWaypointNumber(tp,idxtp);

	// Compute target point pseudo location along trajectory
	findPseudoLoc(tp,idxtp,tploc,ru,nru);

	// Analytic circle solution
	if (strcmp(trajType,"circle")) {
		computeNearestPoint(tploc);
	}

	fw->updateValue("tploc1",tploc[0]);
	fw->updateValue("tploc2",tploc[1]);

	// Get bank angle command
	v1[0] = sv.VNi;	v1[1] = sv.VEi; v1[2] = 0;
	v2[0] = tploc[0]-p[0]; v2[1] = tploc[1]-p[1]; v2[2] = 0;
	v1n = sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
	v2n = sqrt(v2[0]*v2[0]+v2[1]*v2[1]);
	v1[0] = v1[0]/v1n; v1[1] = v1[1]/v1n;
	v2[0] = v2[0]/v2n; v2[1] = v2[1]/v2n;

	cross(v1,v2,tmp);

	if (tmp[2] > 0) {
		eta = 1.0*acos(v1[0]*v2[0]+v1[1]*v2[1]);
	} else {
		eta = -1.0*acos(v1[0]*v2[0]+v1[1]*v2[1]);
	}

	// Desired bank
	phides = atan(2*v1n*v1n/L1*sin(eta)/9.81);

	// Desired airspeed
	Vtarget = ctrlGns.cmap["Vtarget"];// pv.waypointVel[idxtp[0]] + u*(pv.waypointVel[idxtp[1]]-pv.waypointVel[idxtp[0]]);

	// Desired altitude
	dDes = ctrlGns.cmap["dDes"]; //tp[2];

	return phides;
}

void Guidance::computeNearestPoint(double tploc[3]) {

	complex<double> N(sv.Np,0);
	complex<double> E(sv.Ep,0);
	complex<double> Nc(ctrlGns.cmap["Nc"],0);
	complex<double> Ec(ctrlGns.cmap["Ec"],0);
	complex<double> R(ctrlGns.cmap["Rcirc"],0);
	complex<double> L(ctrlGns.cmap["L1gain"],0);
	complex<double> i(0,1.0);
	complex<double> two(2.0,0);
	complex<double> four(4.0,0);
	complex<double> six(6.0,0);
	complex<double> eight(8.0,0);
	complex<double> thd;

 	thd = -log(((E - Ec + N*i - Nc*i)*(E*E - sqrt(E*E*E*E - four*E*E*E*Ec + six*E*E*Ec*Ec -
	    		two*E*E*L*L + two*E*E*N*N - four*E*E*N*Nc + two*E*E*Nc*Nc - two*E*E*R*R -
	    		four*E*Ec*Ec*Ec + four*E*Ec*L*L - four*E*Ec*N*N + eight*E*Ec*N*Nc - four*E*Ec*Nc*Nc +
	    		four*E*Ec*R*R + Ec*Ec*Ec*Ec - two*Ec*Ec*L*L + two*Ec*Ec*N*N - four*Ec*Ec*N*Nc +
	    		two*Ec*Ec*Nc*Nc - two*Ec*Ec*R*R + L*L*L*L - two*L*L*N*N + four*L*L*N*Nc -
	    		two*L*L*Nc*Nc - two*L*L*R*R + N*N*N*N - four*N*N*N*Nc + six*N*N*Nc*Nc -
	    		two*N*N*R*R - four*N*Nc*Nc*Nc + four*N*Nc*R*R + Nc*Nc*Nc*Nc - two*Nc*Nc*R*R +
	    		R*R*R*R) + Ec*Ec - L*L + N*N + Nc*Nc + R*R - two*E*Ec -
	    		two*N*Nc))/(two*R*(E*E - two*E*Ec + Ec*Ec + N*N - two*N*Nc + Nc*Nc)))*i;

	tploc[0] = ctrlGns.cmap["Nc"] + ctrlGns.cmap["Rcirc"]*sin(real(thd));
	tploc[0] = ctrlGns.cmap["Ec"] + ctrlGns.cmap["Rcirc"]*cos(real(thd));
	tploc[2] = ctrlGns.cmap["dDes"];

	return;
}

void Guidance::computeCurvature(int idx[2], double dBdu[3]) {

	double x1, y1, z1, x2, y2, z2, c1x, c1y, c1z, c2x, c2y, c2z;

	// Write out variables in scalar form
	x1 = wp[0][idx[0]];	x2 = wp[0][idx[1]];
	y1 = wp[1][idx[0]]; y2 = wp[1][idx[1]];
	z1 = wp[2][idx[0]]; z2 = wp[2][idx[1]];

	c1x = wtm[0][2*idx[0]]; c2x = wtm[0][2*idx[0]+1];
	c1y = wtm[1][2*idx[0]]; c2y = wtm[1][2*idx[0]+1];
	c1z = wtm[2][2*idx[0]]; c2z = wtm[2][2*idx[0]+1];

	dBdu[0] = x1*pow(u-1.0,2.0)*-3.0-(u*u)*(c2x*3.0+x2*3.0)+pow(u-1.0,2.0)*(c1x*3.0+x1*3.0)+(u*u)*x2*3.0+u*(u*2.0-2.0)*(c1x*3.0+x1*3.0)-u*(u-1.0)*(c2x*3.0+x2*3.0)*2.0;
	dBdu[1] = y1*pow(u-1.0,2.0)*-3.0-(u*u)*(c2y*3.0+y2*3.0)+pow(u-1.0,2.0)*(c1y*3.0+y1*3.0)+(u*u)*y2*3.0+u*(u*2.0-2.0)*(c1y*3.0+y1*3.0)-u*(u-1.0)*(c2y*3.0+y2*3.0)*2.0;
	dBdu[2] = z1*pow(u-1.0,2.0)*-3.0-(u*u)*(c2z*3.0+z2*3.0)+pow(u-1.0,2.0)*(c1z*3.0+z1*3.0)+(u*u)*z2*3.0+u*(u*2.0-2.0)*(c1z*3.0+z1*3.0)-u*(u-1.0)*(c2z*3.0+z2*3.0)*2.0;

	return;
}

void Guidance::findPseudoLoc(double *p, int idx[2], double ploc[3], double ru[3], double nru[3]) {

	double px, py, pz, x1, x2, y1, y2, z1, z2;
	double c1x, c2x, c1y, c2y, c1z, c2z, R, dR;

	// Write out variables in scalar form
	px = *p; py = *(p+1); pz = *(p+2);

	x1 = wp[0][idx[0]];	x2 = wp[0][idx[1]];
	y1 = wp[1][idx[0]]; y2 = wp[1][idx[1]];
	z1 = wp[2][idx[0]]; z2 = wp[2][idx[1]];

	c1x = wtm[0][2*idx[0]]; c2x = wtm[0][2*idx[0]+1];
	c1y = wtm[1][2*idx[0]]; c2y = wtm[1][2*idx[0]+1];
	c1z = wtm[2][2*idx[0]]; c2z = wtm[2][2*idx[0]+1];

	// Perform Newton iteration to converge on u
	u = ul;
	for (int i = 0; i < 4; i++) {
	    // R = r'*dB/du
	    R = -(px+x1*pow(u-1.0,3.0)-(u*u*u)*x2-u*pow(u-1.0,2.0)*(c1x*3.0+x1*3.0)+(u*u)*(u-1.0)*(c2x*3.0+x2*3.0))*(x1*pow(u-1.0,2.0)*3.0+(u*u)*(c2x*3.0+x2*3.0)-pow(u-1.0,2.0)*(c1x*3.0+x1*3.0)-(u*u)*x2*3.0-u*(u*2.0-2.0)*(c1x*3.0+x1*3.0)+u*(u-1.0)*(c2x*3.0+x2*3.0)*2.0)-(py+y1*pow(u-1.0,3.0)-(u*u*u)*y2-u*pow(u-1.0,2.0)*(c1y*3.0+y1*3.0)+(u*u)*(u-1.0)*(c2y*3.0+y2*3.0))*(y1*pow(u-1.0,2.0)*3.0+(u*u)*(c2y*3.0+y2*3.0)-pow(u-1.0,2.0)*(c1y*3.0+y1*3.0)-(u*u)*y2*3.0-u*(u*2.0-2.0)*(c1y*3.0+y1*3.0)+u*(u-1.0)*(c2y*3.0+y2*3.0)*2.0)-(pz+z1*pow(u-1.0,3.0)-(u*u*u)*z2-u*pow(u-1.0,2.0)*(c1z*3.0+z1*3.0)+(u*u)*(u-1.0)*(c2z*3.0+z2*3.0))*(z1*pow(u-1.0,2.0)*3.0+(u*u)*(c2z*3.0+z2*3.0)-pow(u-1.0,2.0)*(c1z*3.0+z1*3.0)-(u*u)*z2*3.0-u*(u*2.0-2.0)*(c1z*3.0+z1*3.0)+u*(u-1.0)*(c2z*3.0+z2*3.0)*2.0);
	    dR = -(py+y1*pow(u-1.0,3.0)-(u*u*u)*y2-u*pow(u-1.0,2.0)*(c1y*3.0+y1*3.0)+(u*u)*(u-1.0)*(c2y*3.0+y2*3.0))*(y1*(u*2.0-2.0)*3.0+(u-1.0)*(c2y*3.0+y2*3.0)*2.0-u*y2*6.0-(u*2.0-2.0)*(c1y*3.0+y1*3.0)*2.0-u*(c1y*3.0+y1*3.0)*2.0+u*(c2y*3.0+y2*3.0)*4.0)-(pz+z1*pow(u-1.0,3.0)-(u*u*u)*z2-u*pow(u-1.0,2.0)*(c1z*3.0+z1*3.0)+(u*u)*(u-1.0)*(c2z*3.0+z2*3.0))*(z1*(u*2.0-2.0)*3.0+(u-1.0)*(c2z*3.0+z2*3.0)*2.0-u*z2*6.0-(u*2.0-2.0)*(c1z*3.0+z1*3.0)*2.0-u*(c1z*3.0+z1*3.0)*2.0+u*(c2z*3.0+z2*3.0)*4.0)-pow(x1*pow(u-1.0,2.0)*3.0+(u*u)*(c2x*3.0+x2*3.0)-pow(u-1.0,2.0)*(c1x*3.0+x1*3.0)-(u*u)*x2*3.0-u*(u*2.0-2.0)*(c1x*3.0+x1*3.0)+u*(u-1.0)*(c2x*3.0+x2*3.0)*2.0,2.0)-pow(y1*pow(u-1.0,2.0)*3.0+(u*u)*(c2y*3.0+y2*3.0)-pow(u-1.0,2.0)*(c1y*3.0+y1*3.0)-(u*u)*y2*3.0-u*(u*2.0-2.0)*(c1y*3.0+y1*3.0)+u*(u-1.0)*(c2y*3.0+y2*3.0)*2.0,2.0)-pow(z1*pow(u-1.0,2.0)*3.0+(u*u)*(c2z*3.0+z2*3.0)-pow(u-1.0,2.0)*(c1z*3.0+z1*3.0)-(u*u)*z2*3.0-u*(u*2.0-2.0)*(c1z*3.0+z1*3.0)+u*(u-1.0)*(c2z*3.0+z2*3.0)*2.0,2.0)-(px+x1*pow(u-1.0,3.0)-(u*u*u)*x2-u*pow(u-1.0,2.0)*(c1x*3.0+x1*3.0)+(u*u)*(u-1.0)*(c2x*3.0+x2*3.0))*(x1*(u*2.0-2.0)*3.0+(u-1.0)*(c2x*3.0+x2*3.0)*2.0-u*x2*6.0-(u*2.0-2.0)*(c1x*3.0+x1*3.0)*2.0-u*(c1x*3.0+x1*3.0)*2.0+u*(c2x*3.0+x2*3.0)*4.0);
	    // Newton step
	    u -= R/dR;
	}
	// Make sure u is between 0 and 1
	u = fmax(fmin(u,1),0);

	ploc[0] = -x2*((u*u)*(u-1.0)*3.0-u*u*u)+x1*(u*pow(u-1.0,2.0)*3.0-pow(u-1.0,3.0))+c1x*u*pow(u-1.0,2.0)*3.0-c2x*(u*u)*(u-1.0)*3.0;
	ploc[1] = -y2*((u*u)*(u-1.0)*3.0-u*u*u)+y1*(u*pow(u-1.0,2.0)*3.0-pow(u-1.0,3.0))+c1y*u*pow(u-1.0,2.0)*3.0-c2y*(u*u)*(u-1.0)*3.0;
	ploc[2] = -z2*((u*u)*(u-1.0)*3.0-u*u*u)+z1*(u*pow(u-1.0,2.0)*3.0-pow(u-1.0,3.0))+c1z*u*pow(u-1.0,2.0)*3.0-c2z*(u*u)*(u-1.0)*3.0;

    ru[0] = px-ploc[0];
	ru[1] = py-ploc[1];
	ru[2] = pz-ploc[2];

	nru[0] = px - ru[0];
	nru[1] = py - ru[1];
	nru[2] = pz - ru[2];

	return;
}

void Guidance::computeWaypointNumber(double *p, int idx[2])  {

	int Nwps = pv.numwps+1;
	int tmp, i, j;
	double d1, d2, minr;
	int* seq = (int*)malloc(sizeof(int)*Nwps);
	double *R = (double*)malloc(sizeof(double)*Nwps);

	// Use last waypoint as initial guess
	tmp = fmax(wpl-1,0);
	for (i = tmp; i < Nwps; i++) {
		*(seq+i) = i;
	}
	for (i = 0; i < tmp; i++) {
		*(seq+i) = i;
	}

	d1 = 0; d2 = 0; idx[0] = -1.0;
	for (j = 0; j < Nwps; j++) {
		i = *(seq+j);
	    // Perform dot product
	    if (i < Nwps) {
	        d1 = (*p-wp[0][i])*wtm[0][2*i]+(*(p+1)-wp[1][i])*wtm[1][2*i]+
	            (*(p+2)-wp[2][i])*wtm[2][2*i];
	        d2 = (*p-wp[0][i+1])*wtm[0][2*i+1]+(*(p+1)-wp[1][i+1])*wtm[1][2*i+1]+
	            (*(p+2)-wp[2][i+1])*wtm[2][2*i+1];
	    }
	    if (d1 >= 0 && d2 >= 0) {
	        if (i < Nwps) {
	        	idx[0] = i;
	        	idx[1] = i+1;
	            break;
	        } else {
	        	idx[0] = Nwps;
	        	idx[1] = 1;
	        }
	    }
	}
	// If dot product technique fails or index jumps
	minr = 1000;
	if (idx[0]==-1) {
		for (i = 0; i < Nwps; i++) {
			*(R+i) = sqrt((*p-wp[0][i])*(*p-wp[0][i])+
					(*(p+1)-wp[1][i])*(*(p+1)-wp[1][i])+
					(*(p+2)-wp[2][i])*(*(p+2)-wp[2][i]));
			if (*(R+i) < minr) {
				idx[0] = i;
				minr = *(R+i);
			}
		}
	}
	idx[1] = idx[0]+1;

	if ((idx[0] >= Nwps) || (idx[0] < wpl && wpl < Nwps-3)) {
		idx[0] = wpl;
		idx[1] = wpl+1;
	}
	free(seq); free(R);
}

void Guidance::computeWaypointTracks() {

	double *r, *At, *bt, *b0, *work, *wt;
    int i, j, k, info, m, n, lwork;

    // Allocate memory
    r   = (double*)malloc(sizeof(double)*pv.numwps*ds*3);
    bt   = (double*)malloc(sizeof(double)*3*pv.numwps*ds*6*pv.numwps);
    At   = (double*)malloc(sizeof(double)*3*pv.numwps*ds*6*pv.numwps);
    b0   = (double*)malloc(sizeof(double)*3*pv.numwps*ds);
    work = (double*)malloc(sizeof(double)*2*6*pv.numwps);
    wt   = (double*)malloc(sizeof(double)*3*2*pv.numwps);

    // Initialize larger matrices to zero
    memset(At,0,sizeof(double)*3*pv.numwps*ds*6*pv.numwps);
    memset(bt,0,sizeof(double)*3*pv.numwps*ds*6*pv.numwps);

    // Generate trajectory (1st input is circle radius)
    genTraj(100, r);

    // Create static matrices to compute Bezier coefficients
    getBezierMatrices(At,bt);

    // Get RHS to compute Bezier coefficients
    getBezierRHS(b0,bt,r);

    // Compute waypoint tracks using least squares
    lwork = 2*6*pv.numwps; m = 3*pv.numwps*ds; n = 6*pv.numwps;
    dgels_("n",&m,&n,&ione,At,&m,b0,&m,work,&lwork,&info);

    if (info != 0) {
        printf("Bezier waypoint track calculation failed in bezierInit.c\n");
    }

    // Reshape into array of waypoint tracks
    k = 0;
    for (j = 0; j < 2*pv.numwps; j++) {
        for (i = 0; i < 3; i++) {
            //*(wt+i*2*pv.numwps+j) = *(b0+k); k++;
            wtm[i][j] = *(b0+k); k++;
        }
    }

    // Record approximately equally spaced waypoints
    for (j = 0; j < pv.numwps; j++) {
    	wp[0][j] = *(r+j*ds);
    	wp[1][j] = *(r+j*ds+pv.numwps*ds);
    	wp[2][j] = *(r+j*ds+2*pv.numwps*ds);
    }
    wp[0][pv.numwps] = *(r+pv.numwps*ds-1);
    wp[1][pv.numwps] = *(r+2*pv.numwps*ds-1);
    wp[2][pv.numwps] = *(r+3*pv.numwps*ds-1);

	// Record waypoints into parameter vector
	for (int i = 0; i < pv.numwps+1; i++) {
		pv.waypointNorth[i] = wp[0][i];
		pv.waypointEast[i] =  wp[1][i];
		pv.waypointAlt[i]  =  wp[2][i];
	}

    // Free memory
    free(r); free(bt); free(At); free(work); free(b0); free(wt);
}

void Guidance::genTraj(double R, double *r) {

    int i, j, k, dt, N;
    double *rx, *ry, *rz, *x, *xi, *rix, *riy, *riz,
    		d1x, d2x, d1y, d2y, d1z, d2z, lon2meter,
    		lat[4], lon[4];

    N = pv.numwps*ds;
    k = 0;

    // Allocate memory
    x   = (double*)malloc(sizeof(double)*5);
    xi  = (double*)malloc(sizeof(double)*N);
    rix = (double*)malloc(sizeof(double)*N);
    riy = (double*)malloc(sizeof(double)*N);
    riz = (double*)malloc(sizeof(double)*N);
    rx  = (double*)malloc(sizeof(double)*17);
    ry  = (double*)malloc(sizeof(double)*17);
    rz  = (double*)malloc(sizeof(double)*17);

    if (strcmp(trajType,"circle") == 0) {
        for (i = 0; i < N; i++) {
            *(r+i) = Nc+Rcirc*sin(2*pi*i/(N-1));
            *(r+i+N) = Ec+Rcirc*cos(2*pi*i/(N-1));
            *(r+i+2*N) = dDes-altDev*cos(8*pi*i/(N-1));
        }
    }
    else if (strcmp(trajType,"straight_north") == 0) {

    }
    else if (strcmp(trajType,"racetrack") == 0) {

    }
    else if (strcmp(trajType,"waypoints") == 0) {

    	lat[0] = 37.3969000000000;
    	lat[1] = 37.3913333333333;
    	lat[2] = 37.3913333333333;
    	lat[3] = 37.3969000000000;

    	lon[0] = 1.221853555555556e+002;
    	lon[1] = 1.221853555555556e+002;
    	lon[2] = 1.221822250000000e+002;
    	lon[3] = 1.221822250000000e+002;

//    	lat[0] = 37.433180555555552;
//    	lat[1] = 37.434102777777774;
//    	lat[2] = 37.433144444444444;
//    	lat[3] = 37.432286111111111;
//    	lon[0] = -1.221835361111111e+002;
//    	lon[1] = -1.221820944444445e+002;
//    	lon[2] = -1.221809638888889e+002;
//    	lon[3] = -1.221825861111111e+002;

    	lon2meter = LAT2METER*cos(apdat.lat*M_PI/180.0);
        dt = floor(N/(5-1));
        for (i = 0; i < 4; i++) {
        	if (i != 3) {
        		d1x = LAT2METER*(lat[i]-pv.initLat);
        		d2x = LAT2METER*(lat[i+1]-pv.initLat);
        		d1y = lon2meter*(lon[i]-pv.initLon);
        		d2y = lon2meter*(lon[i+1]-pv.initLon);
        		if (i == 1) {
        			d1z = dDes+10; d2z = dDes+10;
        		} else {
        			d1z = dDes; d2z = dDes;
        		}
        	} else {
        		d1x = LAT2METER*(lat[i]-pv.initLat);
        		d2x = LAT2METER*(lat[0]-pv.initLat);
        		d1y = lon2meter*(lon[i]-pv.initLon);
        		d2y = lon2meter*(lon[0]-pv.initLon);
        		d1z = dDes;   d2z = dDes;
        	}
        	for (j = 0; j < dt; j++) {
        		*(r+k)     = d1x + ((double)j*(d2x-d1x)/(dt-1));
        		*(r+k+N)   = d1y + ((double)j*(d2y-d1y)/(dt-1));
        		*(r+k+2*N) = d1z + ((double)j*(d2z-d1z)/(dt-1));
        		k++;
        	}
        }
    }
    else if (strcmp(trajType,"racetrack_SWARMS") == 0) {
        dt = floor(N/4);
        for (i = 0; i < 5; i++) {
            *(x+i) = (double)i/4;
        }
        for (i = 0; i < dt; i++) {
            *(xi+i) = (double)i/(dt-1);
        }
        // Straight section #1
        *rx = 0; *(rx+1) = 10; *(rx+2) = 20; *(rx+3) = 30; *(rx+4) = 40;
        *ry = 0; *(ry+1) =  0; *(ry+2) =  0; *(ry+3) =  0; *(ry+4) =  0;
        *rz = 0; *(rz+1) =  0; *(rz+2) =  0; *(rz+3) =  0; *(rz+4) =  0;

        spline(x, rx, xi, rix, 5, dt);
        spline(x, ry, xi, riy, 5, dt);
        spline(x, rz, xi, riz, 5, dt);

        // Store in r
        for (i = 0; i < dt; i++) {
            *(r+i)     = *(rix+i);
            *(r+i+N)   = *(riy+i);
            *(r+i+2*N) = *(riz+i);
        }

        // Turn section #2
        *rx = 40; *(rx+1) = 53.6; *(rx+2) = 58; *(rx+3) = 53.6; *(rx+4) = 40;
        *ry = 0; *(ry+1) = 5.2; *(ry+2) = 16; *(ry+3) = 26.8; *(ry+4) = 32;
        *rz = 0; *(rz+1) =  0; *(rz+2) =  0; *(rz+3) =  0; *(rz+4) =  0;

        spline(x, rx, xi, rix, 5, dt);
        spline(x, ry, xi, riy, 5, dt);
        spline(x, rz, xi, riz, 5, dt);

        // Store in r
        for (i = dt; i < 2*dt; i++) {
            *(r+i)     = *(rix+i-dt);
            *(r+i+N)   = *(riy+i-dt);
            *(r+i+2*N) = *(riz+i-dt);
        }

        // Straight section #3
        *rx = 40; *(rx+1) = 30; *(rx+2) = 20; *(rx+3) = 10; *(rx+4) = 0;
        *ry = 32; *(ry+1) = 32; *(ry+2) = 32; *(ry+3) = 32; *(ry+4) = 32;
        *rz = 0; *(rz+1) =  0; *(rz+2) =  0; *(rz+3) =  0; *(rz+4) =  0;

        spline(x, rx, xi, rix, 5, dt);
        spline(x, ry, xi, riy, 5, dt);
        spline(x, rz, xi, riz, 5, dt);

        // Store in r
        for (i = 2*dt; i < 3*dt; i++) {
            *(r+i)     = *(rix+i-2*dt);
            *(r+i+N)   = *(riy+i-2*dt);
            *(r+i+2*N) = *(riz+i-2*dt);
        }

        // Turn section #4
        *rx = 0; *(rx+1) = -13.6; *(rx+2) = -18; *(rx+3) = -13.6; *(rx+4) = 0;
        *ry = 32; *(ry+1) = 26.8; *(ry+2) = 16; *(ry+3) = 5.2; *(ry+4) = 0;
        *rz = 0; *(rz+1) =  0; *(rz+2) =  0; *(rz+3) =  0; *(rz+4) =  0;

        spline(x, rx, xi, rix, 5, dt);
        spline(x, ry, xi, riy, 5, dt);
        spline(x, rz, xi, riz, 5, dt);

        // Store in r
        for (i = 3*dt; i < 4*dt; i++) {
            *(r+i)     = *(rix+i-3*dt);
            *(r+i+N)   = *(riy+i-3*dt);
            *(r+i+2*N) = *(riz+i-3*dt);
        }
    }
    free(rix); free(riy); free(riz); free(x); free(xi);
}

void Guidance::spline(double *x, double *y, double *xi, double *yi, int n, int ni) {

	double *dx, *divdif, *b, xn, x31, *dl, *d, *du, *dzzdx, *dzdxdx,
            *xl, *coef;
	int i, j, info, *idx;

    // Allocate memory
	dx     = (double*)malloc(sizeof(double)*n-1);
    divdif = (double*)malloc(sizeof(double)*n-1);
    b      = (double*)malloc(sizeof(double)*n);
    d      = (double*)malloc(sizeof(double)*n);
    dl     = (double*)malloc(sizeof(double)*n-1);
    du     = (double*)malloc(sizeof(double)*n-1);
    dzzdx  = (double*)malloc(sizeof(double)*n-1);
    dzdxdx = (double*)malloc(sizeof(double)*n-1);
    coef   = (double*)malloc(sizeof(double)*4*(n-1));
    xl     = (double*)malloc(sizeof(double)*ni);
    idx    = (int*)malloc(sizeof(int)*ni);

    // Compute deltas
    for (i = 0; i < n-1; i++) {
        *(dx+i) = *(x+i+1)-*(x+i);
        *(divdif+i) = (*(y+i+1)-*(y+i))/(*dx);
    }

    // Set up linear system to solve for slopes
    for (i = 1; i < n-1; i++) {
        *(b+i) = 3*(*(dx+i)*(*(divdif+i-1))+*(dx+i-1)*(*(divdif+i)));
    }
    x31 = *(x+2)-*x;
    xn = *(x+n-1)-*(x+n-3);
    *b = ((*dx+2*x31)*(*(dx+1))*(*divdif)+(*dx)*(*dx)*(*(divdif+1)))/x31;
    *(b+n-1) = (*(dx+n-2)*(*(dx+n-2))*(*(divdif+n-3))+(2*xn+(*(dx+n-2)))*
            (*(dx+n-3))*(*(divdif+n-2)))/xn;
    cout << x31 << " " << xn << " " << *dx << " " << *(dx+1) << endl;

    // Compute sparse matrix
    *d = *dx; *(d+n-1) = *(dx+n-3);
    for (i = 1; i < n-1; i++) {
        *(d+i) = 2*(*(dx+i)+*(dx+i-1));
    }
    *dl = x31;
    for (i = 1; i < n-1; i++) {
        *(dl+i) = *(dx+i-1);
        *(du+i-1) = *(dx+i);
    }
    *(du+n-2) = xn;

    // Compute slopes
    dgtsv_( &n, &ione, du, d, dl, b, &n, &info );

    if (info != 0) {
        printf("Spline slope calculation failed in bezierInit.c\n");
    }

    // Compute spline coefficients
    for (i = 0; i < n-1; i++) {
        *(dzzdx+i) = (*(divdif+i)-*(b+i))/(*(dx+i));
        *(dzdxdx+i) = (*(b+i+1)-*(divdif+i))/(*(dx+i));
    }
    for (i =0; i < n-1; i++) {
        *(coef+i) = (*(dzdxdx+i)-*(dzzdx+i))/(*(dx+i));
        *(coef+i+(n-1)) = 2*(*(dzzdx+i))-*(dzdxdx+i);
        *(coef+i+2*(n-1)) = *(b+i);
        *(coef+i+3*(n-1)) = *(y+i);
    }

    // Compute where desired interpolated points fall
    for (i = 0; i < ni; i++) {
        for (j = 0; j < n-1; j++) {
            if ((*(xi+i) >= *(x+j)) &&  (*(xi+i) <= *(x+j+1))) {
                *(idx+i) = j;
            }
        }
    }

    // Go to local coordinates
    for (i = 0; i < ni; i++) {
        *(xl+i) = *(xi+i) - *(x+*(idx+i));
    }

    // Apply nested multiplication to compute spline values
    for (i = 0; i < ni; i++) {
        *(yi+i) = *(coef+*(idx+i));
    }
    for (i = 1; i < 4; i++) {
        for (j = 0; j < ni; j++) {
            *(yi+j) = *(xl+j)*(*(yi+j)) + *(coef+*(idx+j)+i*(n-1));
        }
    }

    free(dx); free(divdif); free(b); free(dl); free(d); free(du);
    free(dzzdx); free(dzdxdx); free(coef); free(idx); free(xl);

	return;

}

void Guidance::getBezierMatrices(double *At, double *bt) {

    double u[ds], A[3][6], b[3][6];
    int i, j, idx1, idx2;

    // Initialize u
    for (i = 0; i < ds; i++) {
        u[i] = (double)i/(ds-1);
    }

    idx1 = 0; idx2 = 0;
    for (i = 0; i < pv.numwps; i++) {
        for (j = 0; j < ds; j++) {
            // Write A
            A[0][0] = 3*u[j]*pow((u[j]-1),2);
            A[1][1] = 3*u[j]*pow((u[j]-1),2);
            A[2][2] = 3*u[j]*pow((u[j]-1),2);
            A[0][3] = -3*pow(u[j],2)*(u[j]-1);
            A[1][4] = -3*pow(u[j],2)*(u[j]-1);
            A[2][5] = -3*pow(u[j],2)*(u[j]-1);
            // Store in larger matrices
            *(At+idx1+idx2*3*pv.numwps*ds)       = A[0][0];
            *(At+idx1+1+(idx2+1)*3*pv.numwps*ds) = A[1][1];
            *(At+idx1+2+(idx2+2)*3*pv.numwps*ds) = A[2][2];
            *(At+idx1+(idx2+3)*3*pv.numwps*ds)   = A[0][3];
            *(At+idx1+1+(idx2+4)*3*pv.numwps*ds) = A[1][4];
            *(At+idx1+2+(idx2+5)*3*pv.numwps*ds) = A[2][5];
            // Write b
            b[0][0] = -pow((u[j]-1),3)+3*u[j]*pow((u[j]-1),2);
            b[1][1] = -pow((u[j]-1),3)+3*u[j]*pow((u[j]-1),2);
            b[2][2] = -pow((u[j]-1),3)+3*u[j]*pow((u[j]-1),2);
            b[0][3] = pow(u[j],3)-3*pow(u[j],2)*(u[j]-1);
            b[1][4] = pow(u[j],3)-3*pow(u[j],2)*(u[j]-1);
            b[2][5] = pow(u[j],3)-3*pow(u[j],2)*(u[j]-1);
            // Store in larger matrices
            *(bt+idx1+idx2*3*pv.numwps*ds)       = b[0][0];
            *(bt+idx1+1+(idx2+1)*3*pv.numwps*ds) = b[1][1];
            *(bt+idx1+2+(idx2+2)*3*pv.numwps*ds) = b[2][2];
            *(bt+idx1+(idx2+3)*3*pv.numwps*ds)   = b[0][3];
            *(bt+idx1+1+(idx2+4)*3*pv.numwps*ds) = b[1][4];
            *(bt+idx1+2+(idx2+5)*3*pv.numwps*ds) = b[2][5];
            idx1 += 3;
        }
        idx2 += 6;
    }
}

void Guidance::getBezierRHS(double *b0, double *bt, double *r) {

    double rv[3*pv.numwps*ds], xp1[6], xp2[6], *C, *wpv;
    int i, idx1, idx2;

    C   = (double*)malloc(sizeof(double)*3*pv.numwps*ds);
    wpv = (double*)malloc(sizeof(double)*6*pv.numwps);

    // Put trajectory into a single vector
    for (i = 0; i < pv.numwps*ds; i++) {
        rv[3*i]   = *(r+i);
        rv[3*i+1] = *(r+i+pv.numwps*ds);
        rv[3*i+2] = *(r+i+2*pv.numwps*ds);
    }

    // Write the difference between the desired trajectory, rv, and the
    // position w/o considering the control points, b
    for (i = 0; i < pv.numwps; i++) {
        xp1[0]=*(r+i*ds);
        xp1[1]=*(r+i*ds+pv.numwps*ds);
        xp1[2]=*(r+i*ds+2*pv.numwps*ds);
        if (i == ds-1) {
            xp2[0]=*(r+(i+1)*ds-1);
            xp2[1]=*(r+(i+1)*ds+pv.numwps*ds-1);
            xp2[2]=*(r+(i+1)*ds+2*pv.numwps*ds-1);
        } else {
            xp2[0]=*(r+(i+1)*ds);
            xp2[1]=*(r+(i+1)*ds+pv.numwps*ds);
            xp2[2]=*(r+(i+1)*ds+2*pv.numwps*ds);
        }
        *(wpv+6*i)   = xp1[0];
        *(wpv+6*i+1) = xp1[1];
        *(wpv+6*i+2) = xp1[2];
        *(wpv+6*i+3) = xp2[0];
        *(wpv+6*i+4) = xp2[1];
        *(wpv+6*i+5) = xp2[2];
    }

    idx1 = 3*pv.numwps*ds; idx2 = 6*pv.numwps;
    dgemv_("n",&idx1,&idx2,&done,bt,&idx1,wpv,&ione,&dzero,C,&ione);

    for (i = 0; i < 3*pv.numwps*ds; i++) {
        *(b0+i) = *(rv+i)-*(C+i);
    }
    free(C); free(wpv);
}

double Guidance::computeAirspeed() {
	double Vb[3], Vbw[3], Va_measured, Va_computed;
	Vb[0] = sv.VNi;  Vb[1]  = sv.VEi; Vb[2]  = sv.VDi;
	Vbw[0] = pv.VNi_wind; Vbw[1] = pv.VEi_wind; Vbw[2] = 0.0;
	inertial2body(sv.psi, sv.theta, sv.phi, Vb);
	inertial2body(sv.psi, sv.theta, sv.phi, Vbw);

	// Compute airspeed
	Va_measured = sqrt((apdat.diffpress-pv.diffb)/0.6125);
	Va_computed = sqrt((Vb[0]-Vbw[0])*(Vb[0]-Vbw[0])+
			           (Vb[1]-Vbw[1])*(Vb[1]-Vbw[1])+
		               (Vb[2]-Vbw[2])*(Vb[2]-Vbw[2]));

	// Do a comparison, if computed is > 3 m/s different, return measured
	if (fabs(Va_computed-Va_measured) > 3) {
		return Va_measured;
	} else {
		return Va_computed;
	}
}

// Takes a vector x in inertial coordinates and converts it to the body frame
void Guidance::inertial2body(double psi, double th, double phi, double *x) {

	double y[3];

	// Local copy of x
	y[0] = x[0];
	y[1] = x[1];
	y[2] = x[2];

	double spsi = sin(psi); double cpsi = cos(psi);
	double sth  = sin(th);  double cth  = cos(th);
	double sphi = sin(phi); double cphi = cos(phi);

	double dcm[3][3];

	dcm[0][0] = cth*cpsi;
	dcm[1][0] = cth*spsi;
	dcm[2][0] = -sth;
	dcm[0][1] = sphi*sth*cpsi - cphi*spsi;
	dcm[1][1] = sphi*sth*spsi + cphi*cpsi;
	dcm[2][1] = sphi*cth;
	dcm[0][2] = cphi*sth*cpsi + sphi*spsi;
	dcm[1][2] = cphi*sth*spsi - sphi*cpsi;
	dcm[2][2] = cphi*cth;

	x[0] = dcm[0][0]*y[0] + dcm[1][0]*y[1] + dcm[2][0]*y[2];
	x[1] = dcm[0][1]*y[0] + dcm[1][1]*y[1] + dcm[2][1]*y[2];
	x[2] = dcm[0][2]*y[0] + dcm[1][2]*y[1] + dcm[2][2]*y[2];

	return;
}

void Guidance::cross(double *a, double *b, double *c) {
	c[0] = a[1]*b[2]-a[2]*b[1];
	c[1] = a[2]*b[0]-a[0]*b[2];
	c[2] = a[0]*b[1]-a[1]*b[0];
	return;
}

void Guidance::GNDdataReceived(Communication &com) {

	if (com.gndmsgtype == MAVLINK_MSG_ID_SET_MODE) {

		//cout << "Commanded mode = " << +com.MAVLINK_MODE << endl;

		//if ((int)com.MAVLINK_MODE==1) {
			//computeWaypointTracks();
		//}
	} else if (com.gndmsgtype == MAVLINK_MSG_ID_PARAM_SET) {

		if ((dDes != ctrlGns.cmap["dDes"]) &&
				strcmp(trajType,"waypoints") == 0) {
			computeWaypointTracks();
		}

/*		// Check to see if circle center has changed
		if (    (Nc     != ctrlGns.cmap["Nc"]) ||
				(Ec     != ctrlGns.cmap["Ec"]) ||
				(Rcirc  != ctrlGns.cmap["Rcirc"]) ||
				(dDes   != ctrlGns.cmap["dDes"])  ||
				(altDev != ctrlGns.cmap["altDev"])) {

			// Circle has changed, need to update trajectory
			Nc     = ctrlGns.cmap["Nc"];
			Ec     = ctrlGns.cmap["Ec"];
			Rcirc  = ctrlGns.cmap["Rcirc"];
			dDes   = ctrlGns.cmap["dDes"];
			altDev = ctrlGns.cmap["altDev"];

			// Compute waypoint locations and Bezier track vectors
			computeWaypointTracks();

			// Record waypoints into parameter vector
			for (int i = 0; i < pv.numwps+1; i++) {
				pv.waypointNorth[i] = wp[0][i];
				pv.waypointEast[i] =  wp[1][i];
				pv.waypointAlt[i]  =  wp[2][i];
			}

			cout << "New circle created" << endl;

		} else if ((vDev != ctrlGns.cmap["vDev"]) ||
				  (Vtarget != ctrlGns.cmap["Vtarget"])) {
			vDev = ctrlGns.cmap["vDev"];
			Vtarget = ctrlGns.cmap["Vtarget"];
			// Set target velocity
			for (int i = 0; i < pv.numwps+1; i++) {
				pv.waypointVel[i] = Vtarget-(vDev*cos(8*pi*double((i-1))/pv.numwps));
			}
			cout << "Velocity command updated" << endl;
		}
		*/
	}

	return;
}

void Guidance::loadGains() {

	int k, MAVID, COMPID;
	string tmp;
	double gain;

	k = 0;
	ifstream Gfile ("qgcgains.txt");
	if (Gfile.is_open()) {
		for (int i = 0; i < 3; i++) {
			getline(Gfile,tmp); // Cycle through header lines
		}
		while (!Gfile.eof() && k < 201) {
			Gfile >> MAVID >> COMPID >> ctrlGns.gainName[k] >> gain;
			ctrlGns.cmap[ctrlGns.gainName[k]] = gain;
			cout << ctrlGns.gainName[k] << " = " << gain << endl;
			k++;
			if (k > 200) {
				cout << "Number of allowable gains (200) exceeded in Controller.cpp" << endl;
				break;
			}
		}
	ctrlGns.numgains = k;
	Gfile.close();
	cout << ctrlGns.numgains << " parameters read from qgcgains.txt" << endl;
	} else {
		cout << "Error opening file" << endl;
	}
}
